#ifndef _ROBOT_CONFIG_H
#define _ROBOT_CONFIG_H
#include "remote_control.h"
#include "arm_math.h"

/******************机器人设置*********************/
// #define INFANTRY
// #define HERO
#define SENTRY

#ifdef SENTRY
#define SENTRY_GIMBAL
// #define SENTRY_CHASSIS
#endif

#ifdef INFANTRY
#define GIMBAL_WITH_IMU
#define OMNIDIRECTIONAL_WHEELS
#endif

#ifdef HERO
#define GIMBAL_WITH_IMU
#define MECANUM_WHEELS
#endif

#ifdef SENTRY_CHASSIS
#define STEERING_WHEELS
#endif

#ifdef SENTRY_GIMBAL
#define GIMBAL_WITHOUT_IMU
#define GIMBAL_WITH_IMU
#endif

#ifdef GIMBAL_WITHOUT_IMU
// #define FULL_ROUNDS //转90度以上（要考虑优弧转动）
#define LIMIT_ROUND //无需考虑优弧转动
// #define ABSOLUTE_LOCATION //上云台按绝对位置处理
#define RELATIVE_LOCATION //上下云台按相对位置处理

#endif

// #define USE_CUSTOM_UI //自定义UI

#define USE_CAN//算法组CAN通信
// #define USE_USART//算法组串口iu

/******************常数设置**************************/
#define ECD_FULL_ROUND 8192  // 一圈的ECD值。实际取值0-8191
#define RC_CH_MAX 671        // 遥控器通道最大值
#define M3508_K (3591.0f/187.0f) //3508减速比
#define ECD_TO_RAD (2.0f*PI/(fp32)ECD_FULL_ROUND) //电机ECD值转弧度
#define RPM_TO_RAD (2.0f*PI/60.0f)//电机速度转弧度每秒
/******************键位设置**************************/
#define CHASSIS_FORTH           KEY_PRESSED_OFFSET_W  // 前进
#define CHASSIS_BACK            KEY_PRESSED_OFFSET_S  //后退
#define CHASSIS_RIGHT_MOVE      KEY_PRESSED_OFFSET_D  //右平移
#define CHASSIS_LEFT_MOVE       KEY_PRESSED_OFFSET_A  //左平移
#define CHASSIS_RIGHT_ROTATE    KEY_PRESSED_OFFSET_E  //右旋转
#define CHASSIS_LEFT_ROTATE     KEY_PRESSED_OFFSET_Q  //左旋转
#define ROBOT_STATE_TO_COMMON   KEY_PRESSED_OFFSET_C  // 切换至普通车模式
#define ROBOT_STATE_TO_SPINNER  KEY_PRESSED_OFFSET_V  // 切换至小陀螺模式
#define BULLET_BASE_OPEN        KEY_PRESSED_OFFSET_Z  //开启弹舱盖
#define BULLET_BASE_CLOSE       KEY_PRESSED_OFFSET_X  //关闭弹舱盖
#define USE_SUPPER_CAP KEY_PRESSED_OFFSET_CTRL //使用超级电容
#define KEY_FRIC_ON KEY_PRESSED_OFFSET_SHIFT //打开摩擦轮

/******************控制量设置*********************/
#ifdef INFANTRY
#define CHASSIS_R 241.66f //单位为mm
#define WHEEL_PERIMETER ((114.0f*PI)/60) //除60是为了mm/s->rpm
#define CHASSIS_THETA 0.7071f
#endif

#ifdef HERO
#define CHASSIS_RX_ADD_RY 343.0f //横纵距离之和
#define WHEEL_PERIMETER ((154.0f*PI)/60) 
        
#endif

#ifdef SENTRY
#define CHASSIS_R 460.83f 
#define WHEEL_PERIMETER ((114.0f*PI)/60) 
#define CHASSIS_THETA 0.7071f
#endif

#ifdef INFANTRY
#define CHASSIS_MAX_SPEED 1500.0f //单位为mm/s
#define CHASSIS_MAX_OMEGA PI   //单位为rad/s
#define CHASSIS_WITH_SUPPER_CAP 2 //使用超级电容时的增益倍数

#define CHASSIS_KEYBOARD_SPEED 900.0f //使用键盘时的速度
#define CHASSIS_KEYBOARD_OMEGA PI //使用键盘时的速度

#endif

#ifdef HERO
#define CHASSIS_MAX_SPEED 6000.0f //单位为mm/s
#define CHASSIS_MAX_OMEGA 1.5f*PI   //单位为rad/s
#define CHASSIS_WITH_SUPPER_CAP 2 //使用超级电容时的增益倍数

#define CHASSIS_KEYBOARD_SPEED 1500.0f //使用键盘时的速度
#define CHASSIS_KEYBOARD_OMEGA 1.5f*PI //使用键盘时的速度

#endif

#ifdef SENTRY
#define CHASSIS_MAX_SPEED 1500.0f //单位为mm/s
#define CHASSIS_MAX_OMEGA PI   //单位为rad/s
#define CHASSIS_WITH_SUPPER_CAP 2 //使用超级电容时的增益倍数

#define CHASSIS_KEYBOARD_SPEED 750.0f //使用键盘时的速度
#define CHASSIS_KEYBOARD_OMEGA PI //使用键盘时的速度

#endif

#define CHASSIS_SPEED_RC -(CHASSIS_MAX_SPEED/RC_CH_MAX)
#define CHASSIS_OMEGA_RC -(CHASSIS_MAX_OMEGA/RC_CH_MAX)
#define CHASSIS_RC_DEADLINE 10  //低于这个值的摇杆读数一律为0

#define YAW_DELTA_MAX_RC 0.01f //在5ms内最大角度改变量(遥控器控制)
#define PITCH_DELTA_MAX_RC 0.005f

#ifdef INFANTRY
#define YAW_DELTA_MAX_MOUSE 0.00012f //在5ms内最大角度改变量(鼠标控制)

#endif

#ifdef SENTRY
#define YAW_DELTA_MAX_MOUSE 0.00012f //在5ms内最大角度改变量(鼠标控制)

#endif

#ifdef HERO
#define YAW_DELTA_MAX_MOUSE 0.00035f //在5ms内最大角度改变量(鼠标控制)

#endif

#define PITCH_DELTA_MAX_MOUSE 0.00001f

#define YAW_RC_SEN      (-YAW_DELTA_MAX_RC/RC_CH_MAX)
#define PITCH_RC_SEN    (-PITCH_DELTA_MAX_RC/RC_CH_MAX)
#define YAW_MOUSE_SEN   -YAW_DELTA_MAX_MOUSE
#define PITCH_MOUSE_SEN -PITCH_DELTA_MAX_MOUSE

#define YAW_MAX_SPEED 15.0f*PI

#define GIMBAL_RC_DEADLINE 1 // 低于这个值的摇杆读数一律为0

#ifdef HERO
#define SHOOT_SPEED_LIMIT 7000.0f
#define FRIC_R 0.03f
#endif
#ifdef INFANTRY
#define SHOOT_SPEED_LIMIT 8000.0f
#define FRIC_R 0.02f
#endif
#ifdef SENTRY
#define SHOOT_SPEED_LIMIT 8000.0f
#define FRIC_R 0.02f
#endif

#ifdef INFANTRY
#define SHOOT_MULTI_FREQUENCY   20       //射击频率：个/s
#define SHOOT_MULTI_TIME_GAP    (1000/SHOOT_MULTI_FREQUENCY)    //连发射击时间间隔
#define SHOOT_TRIGGER_SPEED_LIMIT 6000.0f   //拨弹轮开启时速度 100.0f
#define KEEP_SHOOT_TIME     50    //点射用
#define PRESS_LONG_TIME     1000   //长时间保持

#endif

#ifdef HERO
#define SHOOT_MULTI_FREQUENCY   1       //射击频率：个/s
#define SHOOT_MULTI_TIME_GAP    (1000/SHOOT_MULTI_FREQUENCY)    //连发射击时间间隔
#define SHOOT_TRIGGER_SPEED_LIMIT 4000.0f   //拨弹轮开启时速度 调前32000.0f
#define KEEP_SHOOT_TIME     5   //点射用
#define PRESS_LONG_TIME     1000   //长时间保持

#endif

#ifdef SENTRY
#define SHOOT_MULTI_FREQUENCY   20       //射击频率：个/s
#define SHOOT_MULTI_TIME_GAP    (1000/SHOOT_MULTI_FREQUENCY)    //连发射击时间间隔
#define SHOOT_TRIGGER_SPEED_LIMIT 6000.0f   //拨弹轮开启时速度 100.0f
#define SHOOT_TRIGGER_SPEED_LIMIT_UP 8000.0f   //拨弹轮开启时速度 100.0f
#define KEEP_SHOOT_TIME     50   //点射用
#define PRESS_LONG_TIME     1000   //长时间保持

#endif

/******************遥控器通道设置*********************/
#define HANDLE_LEFT_LR 2   // 遥控器左边摇杆的左右方向  left right
#define HANDLE_LEFT_BF 3   // 遥控器左边摇杆的前后方向  back forth
#define HANDLE_RIGHT_LR 0  // 遥控器右边摇杆的左右方向  left right
#define HANDLE_RIGHT_BF 1  // 遥控器右边摇杆的前后方向  back forth
#define SWITCH_SA 5        // 遥控器两档乒乓开关 SA
#define SWITCH_SB 7        // 遥控器三档乒乓开关 SB
#define SWITCH_SC 6        // 遥控器三档乒乓开关 SC
#define SWITCH_SD 4        // 遥控器两档乒乓开关 SD

#define RC_CHASSIS_X HANDLE_LEFT_BF       // x轴移动
#define RC_CHASSIS_Y_OR_W HANDLE_LEFT_LR  // y轴平移或旋转
#define RC_GIMBAL_YAW HANDLE_RIGHT_LR     // 云台yaw轴
#define RC_GIMBAL_PITCH HANDLE_RIGHT_BF   // 云台pitch轴
#define RC_CHOOSE_Y_OR_W SWITCH_SA        // 选择底盘旋转或平移模式,兼顾哨兵上下云台选择
#define RC_CHOOSE_GIMBAL SWITCH_SA        // 选择受控云台
#define RC_CONTROL_SHOOT SWITCH_SB        // 射击摇杆
#define RC_TOTAL_MODE SWITCH_SC           // 三挡模式开关
#define RC_POWERLESS SWITCH_SD            // 强制无力开关

#endif
